#ifndef POINTCLOUD_DETECT_H
#define POINTCLOUD_DETECT_H

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Point.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <limits>
#include <vector>
#include <random>
#include <chrono>

// 定义2D点云的数据结构
struct Point2D {
    float x;
    float y;
};

typedef std::vector<Point2D> PointCloud2D;
typedef std::shared_ptr<PointCloud2D> PointCloud2DPtr;

// 圆弧结构体
struct ArcSegment {
    float radius;           // 半径
    float startAngle;       // 起始角度
    float endAngle;         // 结束角度
    float confidence;       // 置信度
    float radiusScore;      // 半径评分
    float angleScore;       // 角度评分
    float radialScore;      // 径向分布评分
    std::vector<cv::Point> points; // 圆弧上的点
    cv::RotatedRect ellipse; // 椭圆拟合结果
};
struct LineSegment{
    float length;
    cv::Point2f middlePoint;
    float sseScore;
    float distScore;
    float totalScore;
};

// 常量集中定义区域
// 绘图相关常量
const float OVERLAY_ALPHA = 0.3f;              // 绘制半透明覆盖层的透明度
const float LABEL_DISTANCE_RATIO = 0.7f;       // 标签到中心的距离比例
const int TEXT_LINE_HEIGHT = 20;               // 文本行高
const float TEXT_SCALE = 0.5f;                 // 文本缩放
const float TEXT_INFO_SCALE = 0.4f;            // 信息文本缩放
const int CENTER_POINT_RADIUS = 5;             // 中心点半径
const int CENTER_POINT_BORDER = 7;             // 中心点边框半径
const int INFO_BOX_WIDTH = 120;                // 信息框宽度
const int INFO_BOX_HEIGHT = 165;               // 信息框高度
const int COORD_BOX_WIDTH = 130;               // 坐标框宽度
const int COORD_BOX_HEIGHT = 20;               // 坐标框高度
const int FRAME_INFO_WIDTH = 150;              // 帧信息宽度
const int FRAME_INFO_HEIGHT = 60;              // 帧信息高度
const int FRAME_INFO_TOP = 10;                 // 帧信息顶部边距
const int FRAME_INFO_RIGHT = 160;              // 帧信息右侧边距

// 评分权重
const float RADIUS_SCORE_WEIGHT = 0.15f;       // 半径评分权重
const float ANGLE_SCORE_WEIGHT = 0.2f;        // 角度评分权重 
const float RADIAL_SCORE_WEIGHT = 0.65f;        // 径向分布评分权重





class PointCloudDetect {
private:
    // 随机数生成器
    std::mt19937 rng;
    
    // ROS发布器
    ros::Publisher cross_pub;
    ros::Publisher keypoint_pub;
    ros::Publisher origincloud_pub;
    ros::Publisher halfcloud_pub;  // 新增：用于发布前半部分点云
    
    cv::Point2f lastCloudCenter = cv::Point2f(0, 0);
    bool isFirstCloudCenter = true;
    const float ALPHA = 0.15f;  // 较小的平滑系数，适合2-3像素波动
    
    // 点云范围控制
    size_t cloud_start_idx = 0;  // 新增：点云起始索引
    size_t cloud_end_idx = 166;    // 新增：点云结束索引
    
    // 记忆上一次有效轮廓
    ArcSegment last_valid_arc;   // 保存上一次检测到的有效轮廓
    LineSegment last_valid_line;
    int use_last_arc_count = 0;  // 连续使用上一次轮廓的次数
    bool has_valid_segment = false;  // 是否曾经有过有效轮廓

    
    // 图像边框
    const int border = 10; // 边框宽度为10像素

    // 帧计数
    int frame_count = 0;
    
    // 点云边界
    float min_x, max_x, min_y, max_y;

    // 调试标志
    bool debug_mode = true;

    // 圆弧检测相关参数
    const int MIN_CONTOUR_LENGTH = 100;  // 最小轮廓长度
    const float MIN_RADIUS = 15.0f;      // 最小半径（厘米）
    const float MAX_RADIUS = 22.0f;      // 最大半径（厘米）
    const float TARGET_RADIUS = 20.0f;   // 目标半径（厘米）
    const float MIN_ARC_ANGLE = 2 * M_PI * 2 / 5; // 最小圆弧角度
    const float MAX_ARC_ANGLE = 2 * M_PI * 4 / 5; // 最大圆弧角度
    const float CONFIDENCE_THRESHOLD = 0.55f;      // 置信度阈值

    // 绘制圆弧
    void drawArc(cv::Mat& image, const ArcSegment& arc, const cv::Scalar& color, int thickness = 2);
    cv::Point2f getLowPassSmoothCenter(cv::Point2f& currentCenter);
    // 图像预处理
    void preprocessImage(const cv::Mat& input, cv::Mat& output);
    std::vector<std::vector<cv::Point>> extractContours(const cv::Mat& binary);
    float calculateRadius(const std::vector<cv::Point>& contour, const cv::RotatedRect& ellipse);
    float calculateArcAngle(const std::vector<cv::Point>& contour, const cv::RotatedRect& ellipse);
    float calculateRadialDistribution(const std::vector<cv::Point>& contour, const cv::Point2f& center);
    
    // 辅助函数 - 计算评分
    float calculateScoreFromRange(float value, float targetValue, float minValue, float maxValue);
    
    // 用于椭圆上点的计算
    cv::Point2f calculateEllipsePoint(const cv::Point2f& center, float a, float b, float angle, float rotAngle);
    
    // 调试函数
    void showDebugImage(const cv::Mat& image, const std::string& window_name);
    
    // ProcessImage子函数
    cv::Mat preprocessAndExtractContours(const cv::Mat& image, std::vector<std::vector<cv::Point>>& contours);
    std::vector<std::vector<cv::Point>> filterContours(const std::vector<std::vector<cv::Point>>& contours, const cv::Mat& image);
    std::vector<ArcSegment> detectArcsFromContours(const std::vector<std::vector<cv::Point>>& filteredContours, const cv::Mat& image);
    float calculateArcConfidence(ArcSegment& arc, float pixelRadius, float arcAngle);
    cv::Mat visualizeAndPublishResults(const ArcSegment& arc, const cv::Mat& resultImage);

    // 点云处理
    void rosMsgToPointCloud2D(const sensor_msgs::PointCloud2& cloud_msg, PointCloud2D& cloud);
    void pointCloud2DToRosMsg(const PointCloud2D& cloud, sensor_msgs::PointCloud2& cloud_msg);
    cv::Mat pointCloud2DToImage(const PointCloud2DPtr& cloud);
    cv::Point2f imageToPointCloud(const cv::Point2f& image_point);

    // 更新帧计数
    void updateFrameCount();

    // Line处理
    std::vector<std::vector<cv::Point>> filterContoursLine(const std::vector<std::vector<cv::Point>>& contours, const cv::Mat& image);

    // 公共直线拟合计算
    void fitLineCommon(const std::vector<cv::Point>& points, 
        cv::Vec4f& lineParams, 
        cv::Point2f& refPoint,
        cv::Point2f& dir);
    // 计算投影范围
    void calcProjectionRange(const std::vector<cv::Point>& points,
        const cv::Point2f& refPoint,
        const cv::Point2f& dir,
        float& t_min, float& t_max);
    // 合并后的直线拟合与评估函数
    LineSegment evaluateLine(const std::vector<cv::Point>& points, cv::Mat& image);
    // 统一绘制函数
    void drawResults(cv::Mat& image, 
        const std::vector<cv::Point>& points,
        const cv::Vec4f& lineParams,
        float sse, float dist, float total);
    cv::Point2f getLineMiddlePoint(const cv::Vec4f& lineParams);
    float calculateDistance(const cv::Point2f& p1, const cv::Point2f& p2);
    void refineContourPoints(std::vector<std::vector<cv::Point>>& contours);
    void publishCenterPoint(cv::Point2f& centerPoint, ros::Publisher& pub);
public:
    PointCloudDetect();
    ~PointCloudDetect();

    // 设置点云范围
    void setCloudRange(size_t start_idx, size_t end_idx) {
        cloud_start_idx = start_idx;
        // 如果end_idx小于start_idx，则设为0（表示到末尾）
        cloud_end_idx = (end_idx < start_idx) ? 0 : end_idx;
    }

    // 创建2D点云的函数
    PointCloud2DPtr createPointCloud2D() {
        return std::make_shared<PointCloud2D>();
    }

    // 点云处理函数
    void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg);
    void publishPointCloud(const PointCloud2DPtr& cloud, ros::Publisher& pub);
    void showImage(const cv::Mat& image, const std::string& window_name);
    
    // 图像处理函数
    ArcSegment processImageCircle(const cv::Mat& image);
    LineSegment processImageLine(const cv::Mat& image);


};

#endif 